#include "dynamic_cmd.h"
#include "log/zuclog.h"
#include "motion/mot_priv.h"
#include "errcode.h"
#include "identify_dynamics.h"
#include "dyna/jaka_dyn_intf.h"
#include "axisgroup/collision_detection.h"

#include <math.h>

#define MOTCMD_INFO(fmt, ...) zuclog_info("DYNCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("DYNCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("DYNCMD", fmt, ##__VA_ARGS__)

int CmdSetIdentifyCycleTime::process()
{
    // if (zucmotStatus->dynamics_identifying_status != 0)
    // {
    //     MOTCMD_INFO("Cannot set cycle time until identifying finished!");
    //     return -1;
    // }
    // setIdentifyCycleTime(identify_cycle_time_);
    return 0;
}

int DynCmdStopDynamicsIdentify::process()
{
    MOTCMD_INFO("ZUCMOT_STOP_DYNAMICS_IDENTIFY\n");
    // if (zucmotStatus->dynamics_identifying_status != 0)
    // {
    //     zucmotStatus->dynamics_identifying_status = 0;  // tell control.c to stop
    //     MOTCMD_INFO("Dynamics identification abort!\n");
    // }
    return 0;
}

int DynCmdResetDynamicsIdentify::process()
{
    MOTCMD_INFO("ZUCMOT_RESET_DYNAMICS_IDENTIFY\n");
    // if (zucmotStatus->dynamics_identifying_status == 1)
    // {
    //     MOTCMD_INFO("Dynamics identification still running!\n");
    // }
    // else
    // {
    //     zucmotStatus->dynamics_identifying_status = 0;
    // }
    return 0;
}

int DynCmdCompleteDynamicsIdentify::process()
{
    MOTCMD_INFO("ZUCMOT_COMPLETE_DYNAMICS_IDENTIFY\n");
    // if (zucmotStatus->dynamics_identifying_status == 3)
    // {
    //     zucmotStatus->dynamics_identifying_status = 4;  // tell control.c calculating finished
    //     reportInfo(INFO_DYNAMIC_IDNEITFY_COMPLETE, "Dynamics idenitfy complete");
    // }
    return 0;
}

int DynCmdInCalcDynamicsIdentify::process()
{
    MOTCMD_INFO("ZUCMOT_IN_DYNAMICS_IDENTIFY_CALCULATING\n");
    // if (zucmotStatus->dynamics_identifying_status == 2)
    // {
    //     zucmotStatus->dynamics_identifying_status = 3;  // tell control.c calculating finished
    // }
    return 0;
}

int DynCmdSetDynamicsIdentifyTraj::process()
{
    MOTCMD_INFO("ZUCMOT_SET_DYNAMICS_IDENTIFY_TRAJ\n");
    // for (int i = 0; i < ELE_IDENTI_TRAJ_DIMENSION; i++) { zucmotStatus->dynamics_identify_traj_para[i] = dynamics_identify_traj_para_[i]; }

    // update start pos
    // GetDynamicsIdenitfyStartPos(zucmotStatus->dynamics_identify_traj_para, zucmotStatus->dynamics_identify_start_pos);
    return 0;
}

int CmdStopIdentifyFriction::process()
{
    MOTCMD_INFO("ZUCMOT_STOP_IDENTIFY_FRICTION\n");
    // if (zucmotStatus->friction_identifying_status != 0)
    // {
    //     zucmotStatus->friction_identifying_status = 0;  // tell control.c to stop
    //     resetFrictionTraj();
    //     MOTCMD_INFO("Dynamics identification abort!\n");
    // }
    return 0;
}

int CmdStartIdentifyFriction::process()
{
    MOTCMD_INFO("ZUCMOT_START_IDENTIFY_FRICTION\n");
    if (!zucmotStatus->powered_on || !GET_MOTION_ENABLE_FLAG())
    {
        MOTCMD_INFO("Cannot start Identify in disabled or poweroff!\n");  // @liwang FIXME 增加错误提示信息
        reportError(DYNAMIC_SETTING_STATE_ERROR, "Cannot start Identify in disabled or poweroff");
        return -1;
    }
    // if (zucmotStatus->net_feed_scale < 0.0001)
    // {
    //     zucmotStatus->friction_identifying_status = -1;
    //     MOTCMD_INFO("Can't do movj zucmotStatus->net_feed_scale < 0.0001!\n");
    //     return -1;
    // }
    // if (GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG())
    // {
    //     zucmotStatus->friction_identifying_status = -1;
    //     MOTCMD_INFO("Can't do movj in coordinated mode!\n");
    //     reportError(OPERATION_IN_WRONG_MODE, "Can't do movj in coordinated mode!\n");
    //     return -1;
    // }
    if (zucmotStatus->three_position_enable_limit == 1)
    {
        // zucmotStatus->friction_identifying_status = -1;
        //三位置开关处于打开状态不能运动
        MOTCMD_INFO("Can't jog when the three positions enable limits\n");
        reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't jog when the three positions enable limits");
        return -1;
    }
    // if (zucmotStatus->friction_identifying_status != 0)
    // {
    //     MOTCMD_INFO("Can't start dynamics identification since process is in other state\n");
    //     return -1;
    // }

    double start_pos[ZUCMOT_MAX_JOINTS] = {0};
    double end_pos[ZUCMOT_MAX_JOINTS] = {0};
    // for (int i = 0; i < zucmotConfig->numJoints; i++)
    // {
    //     start_pos[i] = zucmotStatus->identify_friction_start_pos[i + zucmotStatus->friction_identify_joint_index * zucmotConfig->numJoints];
    //     end_pos[i] = zucmotStatus->identify_friction_end_pos[i + zucmotStatus->friction_identify_joint_index * zucmotConfig->numJoints];
    // }

    for (int jntId = 0; jntId < zucmotConfig->numJoints; ++jntId)
    {
        zucmot_joint_t* joint = &joints[jntId];
        /* don't jog past limits */
        if ((end_pos[jntId] > joint->max_pos_limit) || (end_pos[jntId] < joint->min_pos_limit))
        {
            // zucmotStatus->friction_identifying_status = -1;
            reportError(TARGET_POS_EXCEED_LIMIT, "The target position exceeds the mechine's limits.");
            MOTCMD_INFO("friction identify posCmd[%d] = %f, min = %f, max = %f\n", jntId, end_pos[jntId], joint->min_pos_limit, joint->max_pos_limit);
            break;
        }
    }

    // zucmotStatus->friction_identifying_status = 1;
    // zucmotStatus->friction_identify_joint_index = friction_identify_joint_index_;
    return 0;
}

int CmdSetIdentifyFrictionEndPos::process()
{
    short index = friction_identify_joint_index_;
    MOTCMD_INFO("ZUCMOT_SET_IDENTIFY_FRICTION_END_POS %d", index);
    for (int i = 0; i < zucmotConfig->numJoints; i++)
    {
        // zucmotStatus->identify_friction_end_pos[i + index * zucmotConfig->numJoints] = friction_identify_end_pos_[i];
        // MOTCMD_INFO("friction_identify_end_pos[%d] = %f", i, zucmotStatus->identify_friction_end_pos[i + index * zucmotConfig->numJoints]);
    }
    return 0;
}

int CmdSetIdentifyFrictionStartPos::process()
{
    short index = friction_identify_joint_index_;
    MOTCMD_INFO("ZUCMOT_SET_IDENTIFY_FRICTION_START_POS %d", index);
    for (int i = 0; i < zucmotConfig->numJoints; i++)
    {
        // zucmotStatus->identify_friction_start_pos[i + index * zucmotConfig->numJoints] = friction_identify_start_pos_[i];
        // MOTCMD_INFO("friction_identify_start_pos[%d] = %f", i, zucmotStatus->identify_friction_start_pos[i + index * zucmotConfig->numJoints]);
    }
    return 0;
}

int CmdCompleteFrictionIdentify::process()
{
    MOTCMD_INFO("ZUCMOT_COMPLETE_FRICTION_IDENTIFY\n");
    // if (zucmotStatus->friction_identifying_status == 3)
    // {
    //     zucmotStatus->friction_identifying_status = 4;  // tell control.c calculating finished
    //     reportInfo(INFO_FRICTION_IDNEITFY_COMPLETE, "Friction idenitfy complete");
    // }
    return 0;
}

int CmdInFrictionIdentifyCalculating::process()
{
    MOTCMD_INFO("ZUCMOT_IN_FRICTION_IDENTIFY_CALCULATING\n");
    // if (zucmotStatus->friction_identifying_status == 2)
    // {
    //     zucmotStatus->friction_identifying_status = 3;  // tell control.c calculating finished
    // }
    return 0;
}

int CmdSetServoDynamicsPara::process()
{
    if (!zucmotStatus->powered_on)
    {
        MOTCMD_INFO("Servo Parameter should be set in poweron!\n");  // @liwang FIXME 增加错误提示信息
        reportError(DYNAMIC_SETTING_STATE_ERROR, "Cannot set servo dynamics parameters in poweroff state or enbale state");
        return -1;
    }
    MOTCMD_INFO("ZUCMOT_SET_SERVO_DYNAMICS_PARA\n");
    // if (zucmotStatus->dynamics_identifying_status != 4)
    // {
    //     MOTCMD_INFO("Not ready for setting dynamics!\n");
    // }
    for (int i = zucmotConfig->numJoints; i < 60 + zucmotConfig->numJoints; i++)  // @FIXME liwang 7轴参数不对
    {
        zucmotStatus->servo_dynamics_para[i] = para_[i - 6];
        MOTCMD_INFO("%d\n", zucmotStatus->servo_dynamics_para[i]);
    }
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->servo_dynamics_para[i] = DYNAMIC_VERSION; }
    // zucmotStatus->dynamics_identifying_status = 5;
    return 0;
}

int CmdSetDragControlEnable::process()
{
    zucmotStatus->drag_control_setiings.drag_control_enable = drag_control_enable_;
    MOTCMD_INFO("ZUCMOT_SET_DRAG_CONTROL_ENABLE %d\n", zucmotStatus->drag_control_setiings.drag_control_enable);
    return 0;
}

int CmdSetDragControlFcCompensate::process()
{
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->drag_control_setiings.drag_control_fc_compensate[i] = drag_control_fc_compensate_[i]; }
    MOTCMD_INFO("ZUCMOT_SET_DRAG_CONTROL_FC_COMPENSATE %d %d %d %d %d %d\n",
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[0],
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[1],
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[2],
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[3],
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[4],
                zucmotStatus->drag_control_setiings.drag_control_fc_compensate[5]);
    return 0;
}

int CmdSetDragControlFvCompensate::process()
{
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->drag_control_setiings.drag_control_fv_compensate[i] = drag_control_fv_compensate_[i]; }
    MOTCMD_INFO("ZUCMOT_SET_DRAG_CONTROL_FV_COMPENSATE %d %d %d %d %d %d\n",
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[0],
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[1],
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[2],
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[3],
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[4],
                zucmotStatus->drag_control_setiings.drag_control_fv_compensate[5]);
    return 0;
}

int CmdSetDragControlFilterFactor::process()
{
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->drag_control_setiings.drag_control_filter_factor[i] = drag_control_filter_factor_[i]; }
    MOTCMD_INFO("ZUCMOT_SET_DRAG_CONTROL_FILTER_FACTOR %d %d %d %d %d %d\n",
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[0],
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[1],
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[2],
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[3],
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[4],
                zucmotStatus->drag_control_setiings.drag_control_filter_factor[5]);
    return 0;
}

int CmdSetDragControlDitherFactor::process()
{
    MOTCMD_INFO("ZUCMOT_SET_DRAG_CONTROL_DITHER_FACTOR\n");
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->drag_control_setiings.drag_control_dither_factor[i] = drag_control_dither_factor_[i]; }
    return 0;
}

int CmdSetGravityDirection::process()
{
    for (int i = 0; i < 3; i++) { zucmotStatus->gravity_rpy[i] = rpy_[i]; }
    // setGravityRpy(zucmotStatus->gravity_rpy);
    MOTCMD_INFO("ZUCMOT_SET_GRAVITY_DIRECTION, %lf, %lf, %lf\n", zucmotStatus->gravity_rpy[0], zucmotStatus->gravity_rpy[1], zucmotStatus->gravity_rpy[2]);
    return 0;
}

int CmdSetCollisionGmRecordEnable::process()
{
    zucmotStatus->gm_record_enable = gm_record_enable_;
    // enableGmRecord(zucmotStatus->gm_record_enable);
    MOTCMD_INFO("ZUCMOT_SET_COLLISION_GM_RECORD_ENABLE %d\n", zucmotStatus->gm_record_enable);
    return 0;
}

int CmdSetCollisionGmThreshold::process()
{
    for (int i = 0; i < zucmotConfig->numJoints; i++) { zucmotStatus->gm_threshold[i] = gm_threshold_[i]; }
    // setGMPara(zucmotStatus->gm_threshold);
    MOTCMD_INFO("ZUCMOT_SET_COLLISION_GM_THRESHOLD : %lf, %lf, %lf, %lf, %lf, %lf\n",
                zucmotStatus->gm_threshold[0],
                zucmotStatus->gm_threshold[1],
                zucmotStatus->gm_threshold[2],
                zucmotStatus->gm_threshold[3],
                zucmotStatus->gm_threshold[4],
                zucmotStatus->gm_threshold[5]);
    return 0;
}

int CmdSetCollisionFilterPara::process()
{
    MOTCMD_INFO("ZUCMOT_SET_COLLISION_FILTER_PARA %f\n", gm_filter_);
    zucmotStatus->gm_filter = gm_filter_;
    // setGMFilterPara(zucmotStatus->gm_filter);
    return 0;
}

int CmdStartPayloadIdentify::process()
{
    MOTCMD_DEBUG("ZUCMOT_START_PAYLOAD_IDENTIFY");
    if (zucmotStatus->net_feed_scale < 0.0001)
    {
        zucmotStatus->identifying_status = -1;
        zucmotStatus->identifying_trajid = -1;
        return -1;
    }
    if (GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG())
    {
        zucmotStatus->identifying_status = -1;
        zucmotStatus->identifying_trajid = -1;
        reportError(OPERATION_IN_WRONG_MODE, "Can't do START_PAYLOAD_IDENTIFY in coordinated mode!#{\"fixed_key\":[\"TELOP\",\"START_PAYLOAD_IDENTIFY\"]}");
        return -1;
    }
    else
    {
        /*unify the velocity of each joint*/
        int errFlag = 0;
        int jntId = 0;
        double t_max = 0.0;
        double delt_s[ZUCMOT_MAX_JOINTS] = {0};
        zucmotStatus->identifying_trajid = zucmotCommand->id;
        zucmotStatus->identifying_withpayload = zucmotCommand->withpayload.withpayload;
        for (jntId = 0; jntId < zucmotConfig->numJoints; ++jntId)
        {
            double t_this = 0.0;
            // delt_s[jntId] = zucmotCommand->startpayloadidentify.posCmd[jntId] - joints[jntId].free_tp.pos_cmd;
            if ((zucmotStatus->identifying_trajid == 2 && zucmotConfig->robotModel / 10000 == 0) ||
                (zucmotStatus->identifying_trajid == 0 && zucmotConfig->robotModel / 10000 == 1))
            {
                t_this = fabs(delt_s[jntId] / PAYLOAD_IDENTIFY_VAL2);
            }
            else
            {
                t_this = fabs(delt_s[jntId] / PAYLOAD_IDENTIFY_VAL1);
            }
            t_max = t_this > t_max ? t_this : t_max;
        }
        for (jntId = 0; jntId < zucmotConfig->numJoints; ++jntId)
        {
            zucmot_joint_t* joint = &joints[jntId];
            /* don't jog past limits */
            if (((zucmotCommand->startpayloadidentify.posCmd[jntId] > joint->max_pos_limit) &&
                 (fabs(zucmotCommand->startpayloadidentify.posCmd[jntId] - joint->max_pos_limit) > 1e-3)) ||
                ((zucmotCommand->startpayloadidentify.posCmd[jntId] < joint->min_pos_limit) &&
                 (fabs(zucmotCommand->startpayloadidentify.posCmd[jntId] - joint->min_pos_limit) > 1e-3)))
            {
                errFlag = 1;
                zucmotStatus->identifying_status = -1;
                reportError(TARGET_POS_EXCEED_LIMIT, "The target position exceeds the mechine's limits.");
                MOTCMD_INFO("zucmotCommand->startpayloadidentify.posCmd[%d] = %f, min = %f, max = %f\n",
                            jntId,
                            zucmotCommand->startpayloadidentify.posCmd[jntId],
                            joint->min_pos_limit,
                            joint->max_pos_limit);
                break;
            }
        }
        if (errFlag == 0)
        {
            for (jntId = 0; jntId < zucmotConfig->numJoints; ++jntId)
            {
                // joints[jntId].free_tp.pos_cmd = zucmotCommand->startpayloadidentify.posCmd[jntId];
                // joints[jntId].free_tp.max_vel = fabs(delt_s[jntId] / t_max);
                // joints[jntId].free_tp.max_acc = MAX_JOG_JOINT_ACC;
                // joints[jntId].kb_jjog_active = 1;
                // joints[jntId].free_tp.enable = 1;
                SET_JOINT_ERROR_FLAG(&joints[jntId], 0);
                clearHomes(jntId);
            }
            zucmotStatus->identifying_status = 1;
            // zucmotStatus->identifying_trajid = zucmotCommand->id;
            // zucmotStatus->identifying_withpayload = zucmotCommand->withpayload.withpayload;
            SET_MOTION_INPOS_FLAG(0);
        }
    }
    return 0;
}

int CmdResetIdentifyStat::process()
{
    zucmotStatus->identifying_status = 0;
    zucmotStatus->identifying_trajid = -1;
    MOTCMD_INFO("zucmotStatus->identifying_status is reset to 0\n");
    return 0;
}